# -*- coding:utf-8 -*-
"""
作者：ChenTao
日期：2023年03月02日
"""
from serial_port import Serial_port
from plt import MatPlotDraw
import numpy as np


def zhuan(BUFF):
    pos_x = int(float(BUFF[1])) * 1000 + int(float(BUFF[2])) * 100 + int(float(BUFF[3])) * 10 + int(float(BUFF[4]))
    if BUFF[0] == '-':
        pos_x = -pos_x
    pos_y = int(float(BUFF[7])) * 1000 + int(float(BUFF[8])) * 100 + int(float(BUFF[9])) * 10 + int(float(BUFF[10]))
    if BUFF[6] == '-':
        pos_y = -pos_y
    return pos_x, pos_y


mpu = Serial_port(port='COM8', baudrate=115200)
total = 10000
puse = 1
time = 0
x = []
y = []

while total > time:
    try:
        time += puse
        date = mpu.receive()
        if len(date) != 13:
            continue
        # print(date)
        pos_x, pos_y = zhuan(date)
        x.append(pos_x)
        y.append(pos_y)
        print(pos_x, pos_y, time)
    except KeyboardInterrupt:
        mpu.close()

a = MatPlotDraw()
a.Two_Dimensional_Curves(y=y, x_label='t', y_label='y', title='pos_y')
a.Two_Dimensional_Curves(y=x, x_label='t', y_label='x', title='pos_x')
